package ftn.gitis.lidar.filtering;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics2D;
import java.util.Iterator;

import delaunay_triangulation.Delaunay_Triangulation;
import delaunay_triangulation.Point_dt;
import delaunay_triangulation.Triangle_dt;

/**
 * Class that contains static methods that calculates various parameters required in 
 * filtering of LIDAR points using progressive TIN densification.
 * @author Paja
 *
 */
public class TriangleUtils {

	/**
	 * Returns parameters of plane in which a triangle lies.
	 * @param triangle 
	 * @return 4 parameters that describes a plane: ax + by + cz + d = 0
	 */
	public static double[] getTrianglePlane(Triangle_dt triangle) {
		
		//Vector from point p1 to point p2 in form: [x2 - x1]i + [y2 - y1]j + [z2 - z1]k
		double[] v1 = {triangle.p2().x() - triangle.p1().x(), triangle.p2().y() - triangle.p1().y(), triangle.p2().z() - triangle.p1().z()};
		double[] v2 = {triangle.p3().x() - triangle.p1().x(), triangle.p3().y() - triangle.p1().y(), triangle.p3().z() - triangle.p1().z()};
		
		//Normal vector of plane
		double[] n = {v1[1]*v2[2] - v1[2]*v2[1], v1[2]*v2[0] - v1[0]*v2[2], v1[0]*v2[1] - v1[1]*v2[0]}; 
		
		//Final parameters of plane
		double[] result = {n[0], n[1], n[2], n[0]*(-triangle.p1().x()) + n[1]*(-triangle.p1().y()) + n[2]*(-triangle.p1().z())};
		
		return result;
	
	}
	
	/**
	 * Calculates nearest distance between point and a triangle plane.
	 * @param point a point in space.
	 * @param triangle triangle that is part of terrain surface.
	 * @return nearest distance between point and triangle plane.
	 */
	public static double getDistanceFromTriangle(Point_dt point, Triangle_dt triangle) {
		double[] plane = getTrianglePlane(triangle);
		double result = (plane[0] * point.x() + plane[1] * point.y() + plane[2] * point.z() + plane[3])
						/ Math.sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);	
		return result;
	}
	
	/**
	 * Calculates angles between a point and each triangle vertex.
	 * @param point a point in space.
	 * @param triangle triangle that is part of terrain surface.
	 * @return three angles between a point and triangle vertices.
	 */
	public static double[] getAnglesToTriangleVertices(Point_dt point, Triangle_dt triangle) {
		double[] result = new double[3];
		double angle1 = 0;
		double angle2 = 0; 
		double angle3 = 0;
		
		double[] plane = getTrianglePlane(triangle);
		double[] line1 = getLineThrough2Points(point, triangle.p1());
		double[] line2 = getLineThrough2Points(point, triangle.p2());
		double[] line3 = getLineThrough2Points(point, triangle.p3());
		
		angle1 = Math.asin((plane[0]*line1[0] + plane[1]*line1[1] + plane[2]*line1[2]) 
				/ (Math.sqrt(plane[0]*plane[0] + plane[1]*plane[1] +plane[2]*plane[2]) 
				* Math.sqrt(line1[0]*line1[0] + line1[1]*line1[1] + line1[2]*line1[2])));
		angle2 = Math.asin((plane[0]*line2[0] + plane[1]*line2[1] + plane[2]*line2[2]) 
				/ (Math.sqrt(plane[0]*plane[0] + plane[1]*plane[1] +plane[2]*plane[2]) 
				* Math.sqrt(line2[0]*line2[0] + line2[1]*line2[1] + line2[2]*line2[2])));
		angle3 = Math.asin((plane[0]*line3[0] + plane[1]*line3[1] + plane[2]*line3[2]) 
				/ (Math.sqrt(plane[0]*plane[0] + plane[1]*plane[1] +plane[2]*plane[2]) 
				* Math.sqrt(line3[0]*line3[0] + line3[1]*line3[1] + line3[2]*line3[2])));
		
		result[0] = angle1;
		result[1] = angle2;
		result[2] = angle3;
		
		return result;
		
	}
	
	/**
	 * Calculates direction vector of the line that passes through two points.
	 * @param p1 first point
	 * @param p2 second point
	 * @return direction vector of line.
	 */
	public static double[] getLineThrough2Points(Point_dt p1, Point_dt p2) {
		double[] result = {p2.x() - p1.x(), p2.y() - p1.y(), p2.z() - p1.z()}; 
		return result;
	}
	
	/**
	 * Calculates the length of longest edge of triangle. This is necessary for iteration angle reduction for small triangles.
	 * @param triangle 
	 * @return the length of longest edge of triangle.
	 */
	public static double getLongestEdgeLength(Triangle_dt triangle) {
		double l1 = Math.sqrt((triangle.p2().x() - triangle.p1().x()) * (triangle.p2().x() - triangle.p1().x())
					+ (triangle.p2().y() - triangle.p1().y()) * (triangle.p2().y() - triangle.p1().y()));
		double result = l1;
		if(!triangle.isHalfplane()) {
			double l2 = Math.sqrt((triangle.p3().x() - triangle.p2().x()) * (triangle.p3().x() - triangle.p2().x())
				+ (triangle.p3().y() - triangle.p2().y()) * (triangle.p3().y() - triangle.p2().y()));
			if(l2 > result) result = l2;
			double l3 = Math.sqrt((triangle.p1().x() - triangle.p3().x()) * (triangle.p1().x() - triangle.p3().x())
				+ (triangle.p1().y() - triangle.p3().y()) * (triangle.p1().y() - triangle.p3().y()));
			if(l3 > result) result = l3;
		}
		return result;
	}
	
	public static LidarPoint findMirrorPoint(Triangle_dt triangle, LidarPoint originalPoint) {
		LidarPoint result;
		Point_dt closestVertex;
		double[] vector;
		double dist1 = originalPoint.distance3D(triangle.p1());
		double dist2 = originalPoint.distance3D(triangle.p2());
		double dist3;
		if (triangle.isHalfplane())
			dist3 = Double.MAX_VALUE;
		else
			dist3 = originalPoint.distance3D(triangle.p3());
		if((dist1 < dist2) && (dist1 < dist3)) 
			closestVertex = triangle.p1();
		else if (dist2 < dist3)
			closestVertex = triangle.p2();
		else
			closestVertex = triangle.p3();
		
		vector = getLineThrough2Points(originalPoint, closestVertex);
		
		result = new LidarPoint(originalPoint.x() + 2 * vector[0], originalPoint.y() + 2 * vector[1], originalPoint.z() + 2 * vector[2], 1);
		
		return result;
	}
	
	public static void drawTriangles(Graphics2D g, Delaunay_Triangulation triangulation) {
		/* Bounding box of triangulation */
		double xObject1 = triangulation.bb_min().x(); //west 
		double yObject1 = triangulation.bb_max().y(); //north
		
		/* 1 pixel = 0.5m */
		double xScale = 0.5D;
		double yScale = 0.5D;
		
		/* Calculated screen coordinates */
		int xScreen1;
		int yScreen1;
		int xScreen2;
		int yScreen2;
		int xScreen3;
		int yScreen3;
		
		Triangle_dt currentTriangle = null;
		Iterator<Triangle_dt> iterator = triangulation.trianglesIterator();
		while(iterator.hasNext()) {
			currentTriangle = iterator.next();
			if(!currentTriangle.isHalfplane()) {
			xScreen1 = (int)Math.round((currentTriangle.p1().x() - xObject1) / xScale);
			yScreen1 = (int)Math.round((currentTriangle.p1().y() - yObject1) / yScale);
			xScreen2 = (int)Math.round((currentTriangle.p2().x() - xObject1) / xScale);
			yScreen2 = (int)Math.round((currentTriangle.p2().y() - yObject1) / yScale);
			xScreen3 = (int)Math.round((currentTriangle.p3().x() - xObject1) / xScale);
			yScreen3 = (int)Math.round((currentTriangle.p3().y() - yObject1) / yScale);
			
			g.setStroke(new BasicStroke());
			g.setColor(Color.YELLOW);
			g.drawLine(xScreen1, yScreen1, xScreen2, yScreen2);
			g.drawLine(xScreen2, yScreen2, xScreen3, yScreen3);
			g.drawLine(xScreen3, yScreen3, xScreen1, yScreen1);
			}
		}
		
	}
}
